#!/usr/bin/python3
# -*- coding:utf-8 -*-
from datetime import datetime
import os
import serial
import cv2
import configparser
import pickle
import time
import binascii
import numpy
import socket

import sys
sys.path.append("./")
sys.path.append("../")

from vision.Pretreat import Pretreat
from vision.ClassifierVersion import Img2Status
import vision.Camera as Camera

SOLVE_SERVER_HOST = "127.0.0.1"
SOLVE_SERVER_PORT = 2077

def is_start():
    ser = serial.Serial('/dev/ttyS0', 115200)
    while True:
        size = ser.inWaiting()               # 获得缓冲区字符
        if size != 0:
            response = ser.read(size)# 读取内容并显示
            response = binascii.b2a_hex(response).decode("utf-8")
            #print(response)
            if response == '4f':
                print("r")
                break

def rotate_finish():
    ser = serial.Serial('/dev/ttyS0', 115200)
    while True:
        size = ser.inWaiting()               # 获得缓冲区字符
        if size != 0:
            response = ser.read(size)# 读取内容并显示
            response = binascii.b2a_hex(response).decode("utf-8")
            if response == '4f':
                break
            # elif response == "50":
               # break

def getSolution(facelet_str, solve_type=0, time_limit=5):
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
        sock.connect((SOLVE_SERVER_HOST, SOLVE_SERVER_PORT))
        sock.sendall(bytes( ("%d %d %s\n" % (solve_type, time_limit, facelet_str) ).encode('utf-8') ))
        solution = sock.recv(1024)
    solution = str(solution, encoding='utf-8')
    return solution

def getFinalResult(str_result):
    java_res = os.popen(
        "echo \"{0}\" | ../transform/build/transform"
        .format(str_result))
    lis = java_res.read()
    index_ff = lis.index('ff')
    finalRes = lis[index_ff:-7]+'0'
    return finalRes

def trans(s):
    lis = []
    tran = ''
    for word in s.split():
        if len(word) == 1:
            word = '0' + word
            lis.append(word)
        else:
            lis.append(word)
    for element in lis:
        tran = tran + element + ' '
    tran = tran[:-1]
    return tran

def nothing():
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 01 56 56 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame2():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 03 59 6d ae 74 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame1():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 05 d2 e5 19 6d ae eb 00')
    ser.write(hex_str)
    ser.close()

def rotate_frame3():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 03 d2 e5 16 cd 00')
    ser.write(hex_str)
    ser.close()

def rotate_backtostart():
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex('ff ff 07 59 6d a2 e5 19 6d ae 81 00')
    ser.write(hex_str)
    ser.close()

def rotate_fuyuan(move):
    '''
    Deprecated
    '''
    ser = serial.Serial("/dev/ttyS0", 115200)
    hex_str = bytes.fromhex(move)
    ser.write(hex_str)
    ser.close()

def request_frame(sock):
    '''
    Deprecated
    '''
    sock.send(b'##') #发送请求
    time.sleep(0.5)
    img1 = cv2.imread("server1.jpg")
    img2 = cv2.imread("server2.jpg")
    return  img1,img2

# we may set the default values
# sig_cap = false, sig_ok = false, sig_term = false
# we use pipe to transfer images
# captured image size must be consistent with that used in calibration
def camera_proc(conn, sig_cap, sig_ok, sig_term):
    #
    # Read Camera specific params
    #
    cam_idx_list, calib_mtx, calib_dist = Camera.load_camera_params()
    #
    # Init camera
    #
    caps = [cv2.VideoCapture(i) for i in cam_idx_list]

    while True:
        stat = [cap.isOpened() for cap in caps]
        if False not in stat:
            break

    for cap in caps:
        cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M','J','P','G'))
        cap.set(cv2.CAP_PROP_FRAME_WIDTH, Camera.WIDTH)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT, Camera.HEIGHT)

    sig_ok.value = True
    print("camera ok")
    _itr = 0
    while True:
        frames = [cap.read()[1] for cap in caps]
        # print("cycle", _itr)
        _itr += 1
        if sig_cap.value == True:
            print("capture!")
            images = [frame.copy() for frame in frames]
            images = [Camera.camera_calibration(img, calib_mtx, calib_dist) for img in images]
            conn.send(images)
            sig_cap.value = False
        if sig_term.value == True:
            break
    print("release captures...")
    for cap in caps:
        cap.release()

# Let us set the faces 
#   U               Yellow
# L F R B  <=> Blue   Red   Green Orange
#   D                White
#
# and Follow the Disordering Formular D' F2 R' L' B' U' L' B2 L' F B' D' L D2 L'
# We get the six faces
#      L F F 
#      B U D 
#      B D F 
#D L L U L U L R L D D B 
#B L L U F R F R B D B R 
#D U B D U U R U R U L R 
#      R B F 
#      R D F 
#      F F B 
#
# which could be flattened to
# LFFBUDBDFLRLFRBRURULUUFRDUURBFRDFFFBDLLBLLDUBDDBDBRULR
#

def main():
    print("now satrt")
    pics = []
    config = configparser.ConfigParser()
    config.read("../configs/vision_pretreat.ini")
    pretreater = Pretreat(config)
    solver = Img2Status("../vision/ClfTrain/now.model")
    print('configuration and model loaded')

#    is_start()
#    print("hardware waiting")
#    time.sleep(1)

    # Actually, I found the original design quite sophisticated
    # I have to use another main loop to take care of the errors
    while True:
        print("loop solve......")
        is_start()
        # min2phase part
        print("min2phase start")
        start_time = datetime.now()
        # cube_status = solver.status
        cube_status = "FUURULBRULFLBRFRDDRUBDFRBBULLFLDBDFBDFULLRRUDFBLDBDRUF"
#       print(cube_status)
        
        print("U:",cube_status.count("U"),end=" ")
        print("D:",cube_status.count("D"),end=" ")
        print("R:",cube_status.count("R"),end=" ")
        print("L:",cube_status.count("L"),end=" ")
        print("F:",cube_status.count("F"),end=" ")
        print("B:",cube_status.count("B"))

#        min2phase_res = getSolution(cube_status)
#        print(min2phase_res)
        min2phase_res = "B2 D2 R  F2 R  U2 L' R  D2 U2 R' B2 D' L2 U  F2 R  B  F  U  F2"
        moves = getFinalResult(min2phase_res)

        print("step number:\t\t",int(moves.split()[2],16)*2)
        if int(moves.split()[2],16)*2 == 456:
            solver.ToPics(pretreater.perspectived_imgs)
            #raise OSError
            nothing()
            continue
        #print(moves)
        #final_moves = trans(moves)
        #print(final_moves)
        #rotate_fuyuan(final_moves)
        print("min2phase cost:\t\t", (datetime.now() - start_time))
        rotate_finish()
        print("total cost:\t\t",(datetime.now()-start_time))


if __name__ == '__main__':
    main()
